#include "Ctrl.h"
#include "sys.h"
#include "hmi_driver.h"
#if !defined(MIN)
#define MIN(a, b) (((a) < (b)) ? (a) : (b))
#endif
#define ERROR_LOCATION 3.0f//误差范围
#if !defined(MAX)
#define MAX(a, b) (((a) > (b)) ? (a) : (b))
#endif

/*
三档速度控制策略
*/
u8 flagL = 0;//长途		100以上距离
u8 flagM = 0;//中途   25~100 
u8 flagS = 0;//短途   0~25
/*
加速度控制
*/
u8 flag_acce = 0;

float motor_0_kp = 20, motor_0_ki=0.18, motor_0_kd=0;//RF
float motor_1_kp = 20, motor_1_ki=0.18, motor_1_kd=0;//LF
float motor_2_kp = 20, motor_2_ki=0.18, motor_2_kd=0;//LB
float motor_3_kp = 20, motor_3_ki=0.18, motor_3_kd=0;//RB
float position_kp = 15.5, position_ki = 6.5, position_kd = 32.0;

//float roll_kp = 10, roll_ki, roll_kd=3;
float roll_kp = 35, roll_ki = 0, roll_kd = 138;

float go_angle;                                                          // 前进方向与车身夹角，单位为度
float Roll_angle=0;                                                        // 车模需要旋转的角度，单位为度
float Roll_target, Roll_error, Roll_error0;                              // 自传所需传入数据
float error_pisition, error_pisition0;                                   // 定位所需传入数据
int set_speed = 0, set_speed_y = 0, set_speed_x = 0, set_speed_roll = 0; // 总速度，Y轴速度，X轴速度，旋转速度
int set_speed_LF, set_speed_LB, set_speed_RF, set_speed_RB;                  // 四轮设定速度

// 0：右前，1：左前，2：右后，3左后。（根据实际更改）
int pwm_0, pwm_1, pwm_2, pwm_3;
int motor_0_error_0 = 0, motor_0_error_1 = 0, motor_0_error_2 = 0;
int motor_1_error_0 = 0, motor_1_error_1 = 0, motor_1_error_2 = 0;
int motor_2_error_0 = 0, motor_2_error_1 = 0, motor_2_error_2 = 0;
int motor_3_error_0 = 0, motor_3_error_1 = 0, motor_3_error_2 = 0;
int error, error0;

unsigned char stop = 1;

//--------输出参数--------//
float speed_x, speed_y;
float move_x, move_y, move_k1=0.153f, move_k2=0.153f;
float Speed_RF, Speed_LF, Speed_LB, Speed_RB;
int speed_rf, speed_lf, speed_lb, speed_rb;
int temp_rf, temp_lf, temp_lb, temp_rb;
int pwm_0_out, pwm_1_out, pwm_2_out, pwm_3_out;

float X_pre = 0, Y_pre = 0;
const double tran_num = 3.1415926 / 180;

u8 mode_1 = 0;//粗调
u8 mode_2 = 0;//精调

void motor_init(void)
{
	Motor1_GPIO_Config();
	Motor2_GPIO_Config();
	Motor3_GPIO_Config();
	Motor4_GPIO_Config();
	TIM5_PWM_Init(10000-1,1-1);
	TIM10_PWM_Init(10000-1,1-1);
	encoder_init();
	//舵机
	sero_pwm_Init();
	EN_Motoer_ON;									// 开启电机驱动板
	stop = 0;
}

/*
根据路程误差得出目标速度
输入:error 与 error0
*/
float set_y,set_x;
unsigned int move_speed = 0, last_speed = 0;
unsigned int speed_min = HIGH1, speed_max = HIGH2+50; 
unsigned int Imax = 0, lim_I = 40;
const float position_alpha = 0.75;
static inline unsigned int position_PID_contorl(float error, float error_0)//位置式PID+积分限幅
{
	static u8 tL = 0;//加速时间 0.4s
	/*减速控制*/
	if(flagL)//长途高速模式
	{
		if(flag_acce == 0)
		{
			tL++;
			if(tL<180){speed_min = HIGH1;}
			if(tL == 180){tL=0;speed_min = HIGH2;flag_acce=1;}
		}
		else//减速
		{
			if(error <= 30){speed_min = HIGH0;}
			if(error <= 15){speed_min = LOW0;}
			if(error <= 3)
			{
				error=0;Set_action(0,0,Roll_target);motor_0_kp = 30;motor_1_kp = 30;motor_2_kp = 30;motor_3_kp = 30;
				motor_0_ki = 1;motor_1_ki = 1;motor_2_ki = 1;motor_3_ki = 1;
			}
		}
	}
	else if(flagM)//中途中速模式
	{
		if(flag_acce == 0)
		{
			tL++;
			if(tL<120){speed_min = HIGH0;}//500ms加速
			if(tL == 120){tL=0;speed_min = HIGH1;flag_acce=1;}
		}
		else//减速
		{
			if(error <= 25){speed_min = HIGH0;}
			if(error <= 20){speed_min = LOW0;}
			if(error <= 3)
			{
				error=0;Set_action(0,0,Roll_target);motor_0_kp = 30;motor_1_kp = 30;motor_2_kp = 30;motor_3_kp = 30;
				motor_0_ki = 1;motor_1_ki = 1;motor_2_ki = 1;motor_3_ki = 1;
			}
		}
	}
	else if(flagS)
	{
		speed_min = HIGH0;
	}
	
	move_speed = (int)(position_kp * error + position_kd * (error - error_0));
	Imax += (int)(position_ki * error);
	Imax = MIN(lim_I, Imax);
	move_speed += Imax;
	move_speed = MAX(move_speed, speed_min);
	move_speed = MIN(move_speed, speed_max);
	move_speed = (int)(last_speed * (1 - position_alpha) + move_speed * position_alpha);//速度滤波
	last_speed = move_speed;
	return move_speed;
}
/*
位置闭环实现
*/
action_enum action_flag = NONE;
static inline void get_to_position()
{
	if(action_flag!=WAYING)return;
	float temp_x, temp_y, temp_angle;
	int temp_speed = 0;
	temp_x = set_x - X_pre;//这里不知为何 原X_pre - set_x
	temp_y = set_y - Y_pre;
//	temp_angle = atan2(temp_x, temp_y) * 180 / 3.1415926;//这里可能得改
	temp_angle = atan2(temp_y, temp_x) * 180 / 3.1415926;
	temp_angle -= yaw;//统一小车与绝对坐标
	error_pisition = sqrt((temp_x * temp_x) + (temp_y * temp_y));//计算误差
//	if(error_pisition <= 40){flag3 = 0;}
//	else if(error_pisition <= 210){flag3 = 1;}
	if (error_pisition >= ERROR_LOCATION)//误差先范围
	{
		temp_speed = position_PID_contorl(error_pisition, error_pisition0);//目标速度计算
		error_pisition0 = error_pisition;
	}
	else
	{
//		motor_0_kp = 50;motor_1_kp = 50;motor_2_kp = 50;motor_3_kp = 50;
//		position_kp=0.006;//更新
		temp_speed = 0;
		action_flag = IN_POSITION;
	}
	Set_action(temp_speed, temp_angle, Roll_target);
}
/**/
static inline void Roll_to_Target()
{
	if(action_flag != ROLLING)return;
	Set_action(0,0,Roll_target);
	if(fabsf(Roll_target - yaw)<1)
		action_flag = IN_POSITION;
	return;
}
/*
开始:一进入就会先进入的一个空闲状态机,红灯闪烁
不空闲绿灯闪烁
*/
static inline void Start()
{
	if(action_flag == NONE)
	{
		LED1 = !LED1;
		return;
	}
	else if(action_flag == LEISURE)
	{
		LED0 = !LED0;
	}
	return;
}
/*更新路程*/
static inline void Updata_location()
{
    float dx = speed_x * 0.005f;//5ms中断
    float dy = speed_y * 0.005f;
		double ag = yaw * tran_num;//转化为弧度
    double s = sin(ag), c = cos(ag);
		X_pre += -s * dy + c * dx;//这里? -->VX = sVx + cVy   VY = sVy - cVx 原: -s * dy + c * dx
    Y_pre += s * dx + c * dy;//原s * dx + c * dy
}
/*
获取速度
*/
/**
  * @brief 计算校验和
  * @param ptr：需要计算的数据
  * @param len：需要计算的长度
  * @retval 校验和
  */
static inline uint8_t check_sum(uint8_t init, uint8_t *ptr, uint8_t len )
{
	/*校验和的计算结果*/
	uint8_t sum = init;

	while(len--)
	{
		sum += *ptr;/*依次累加各个数据的值*/
		ptr++;
	}

	return sum;
}

static inline void set_computer_value(uint8_t cmd, uint8_t ch, void *data, uint8_t num)
{
	packet_head_t set_packet;

	uint8_t sum = 0;    // 校验和
	num *= 4;           // 一个参数 4 个字节

	set_packet.head = FRAME_HEADER;     // 包头 0x59485A53
	set_packet.ch   = ch;              // 设置通道
	set_packet.len  = 0x0B + num;      // 包长
	set_packet.cmd  = cmd;             // 设置命令

	sum = check_sum(0, (uint8_t *)&set_packet, sizeof(set_packet));       // 计算包头校验和
	sum = check_sum(sum, (uint8_t *)data, num);                           // 计算参数校验和

	usart1_send((uint8_t *)&set_packet, sizeof(set_packet));    // 发送数据头
	usart1_send((uint8_t *)data, num);                          // 发送参数
	usart1_send((uint8_t *)&sum, sizeof(sum));                  // 发送校验和
}

static inline void Get_speed(float alpha)
{
		int temp = 0;
		temp = TIM2->CNT;
		TIM2->CNT = 30000;
    speed_lb = temp - 30000;//编码器修正值
	
		temp = TIM8->CNT;
		TIM8->CNT = 30000;
    speed_rb = -(temp - 30000);
	
		temp = TIM4->CNT;
		TIM4->CNT = 30000;
    speed_lf = temp - 30000;
	
		temp = TIM1->CNT;
		TIM1->CNT = 30000;
    speed_rf = -(temp - 30000);
	
    Speed_RB = speed_rb * alpha + temp_rb * (1 - alpha);//RB
    Speed_LB = speed_lb * alpha + temp_lb * (1 - alpha);//LB 
    Speed_LF = speed_lf * alpha + temp_lf * (1 - alpha);//LF
    Speed_RF = speed_rf * alpha + temp_rf * (1 - alpha);//RF

    temp_rb = (int)Speed_RB;
    temp_lb = (int)Speed_LB;
    temp_lf = (int)Speed_LF;
    temp_rf = (int)Speed_RF;	
		
//		printf("LF:%.2f\r\n",Speed_LF);
//		printf("RF:%.2f\r\n",Speed_RF);
//		printf("LB:%.2f\r\n",Speed_RF);
//		printf("RB:%.2f\r\n",Speed_RB);
}


/*
紧急制动
*/
static inline void Get_Tspeed()
{
    if (stop)
    {
        set_speed_LF = 0;
        set_speed_LB = 0;
        set_speed_RF = 0;
        set_speed_RB = 0;
    }
    else
    {
//        set_speed_x = (int)(set_speed * sin(go_angle / 180 * 3.14159));
//        set_speed_y = (int)(set_speed * cos(go_angle / 180 * 3.14159));
				set_speed_x = (int)(set_speed * cos(go_angle / 180 * 3.14159));
				set_speed_y = (int)(set_speed * sin(go_angle / 180 * 3.14159));
        set_speed_LF = set_speed_y + set_speed_x - set_speed_roll;//LF
        set_speed_LB = set_speed_y - set_speed_x - set_speed_roll;//LB
        set_speed_RF = set_speed_y - set_speed_x + set_speed_roll;//RF
        set_speed_RB = set_speed_y + set_speed_x + set_speed_roll;//RB
    }
}
/*
move_k1 ->防止车轮打滑,提高适应性
*/
static inline void Get_xyspeed()
{
    speed_y = ((Speed_RF + Speed_LF + Speed_LB + Speed_RB) * 0.25) * move_k1;
    speed_x = ((-Speed_RF + Speed_LF - Speed_LB + Speed_RB) * 0.25) * move_k2;//Speed_1 + Speed_3 - Speed_2 - Speed_0
}

#define PWMOUT_MAX 9900
/*设置电机正反转*/
void Set_one_pwm(int temp)
{
	if(temp>=0)
	{
		AN1_Low;
		TIM5->CCR2 = temp;
	}
	else
	{
		AN1_High;
		TIM5->CCR2 = ARR+temp;
	}
}	

void Set_two_pwm(int temp)
{
	if(temp>=0)
	{
		AN2_Low;
		TIM5->CCR3 = temp;
	}
	else
	{
		AN2_High;
		TIM5->CCR3 = ARR+temp;
	}
}	

void Set_three_pwm(int temp)
{
	if(temp>=0)
	{
		AN3_Low;
		TIM5->CCR4 = temp;
	}
	else
	{
		AN3_High;
		TIM5->CCR4 = ARR+temp;
	}
}	

void Set_four_pwm(int temp)
{
	if(temp>=0)
	{
		AN4_High;
		TIM10->CCR1 =  ARR-temp;
		
	}
	else
	{
		AN4_Low;
		TIM10->CCR1 = -temp;
	}
}	

void swerve(float x, float y, float angle)
{
	set_x = x;
	set_y = y;
	Roll_target = angle;
	motor_0_kp = P;motor_1_kp = P;motor_2_kp = P;motor_3_kp = P;
	action_flag = WAYING;
}
//PWM输出（含限幅）
static inline void pwm_out()
{

    if (pwm_0_out >= PWMOUT_MAX)
        pwm_0_out = PWMOUT_MAX;
    else if (pwm_0_out <= -PWMOUT_MAX)
        pwm_0_out = -PWMOUT_MAX;

    if (pwm_1_out >= PWMOUT_MAX)
        pwm_1_out = PWMOUT_MAX;
    else if (pwm_1_out <= -PWMOUT_MAX)
        pwm_1_out = -PWMOUT_MAX;

    if (pwm_2_out >= PWMOUT_MAX)
        pwm_2_out = PWMOUT_MAX;
    else if (pwm_2_out <= -PWMOUT_MAX)
        pwm_2_out = -PWMOUT_MAX;

    if (pwm_3_out >= PWMOUT_MAX)
        pwm_3_out = PWMOUT_MAX;
    else if (pwm_3_out <= -PWMOUT_MAX)
        pwm_3_out = -PWMOUT_MAX;
		
		#if STOP
			pwm_0_out = pwm_1_out=pwm_2_out=pwm_3_out=0;
			
		#endif	
		// 输出pwm
		Set_four_pwm(pwm_2_out);//LB
		Set_three_pwm(pwm_3_out);//RB
		Set_two_pwm(pwm_0_out);//RF
		Set_one_pwm(pwm_1_out);//LF
}


int roll_max = 850;
//转向PID控制
static inline void Roll_PID_Control()
{
    Roll_error = Roll_target - yaw;
    if (Roll_error > 180)
        Roll_error = Roll_error - 360;
    if (Roll_error <= -180)
        Roll_error = 360 + Roll_error;
    set_speed_roll = (int)(roll_kp * Roll_error + roll_kd * (Roll_error - Roll_error0));
    Roll_error0 = Roll_error;
    if(set_speed_roll>roll_max)set_speed_roll=roll_max;
		else if(set_speed_roll+roll_max<0)set_speed_roll=0-roll_max;
}

static inline void PID_Control()
{
    motor_0_error_0 = set_speed_RF - (int)Speed_RF;
    pwm_0 = (int)((motor_0_error_0 - motor_0_error_1) * motor_0_kp + motor_0_error_0 * motor_0_ki + (motor_0_error_0 - 2 * motor_0_error_1 + motor_0_error_2) * motor_0_kd);
    pwm_0_out += pwm_0;
    motor_0_error_2 = motor_0_error_1;
    motor_0_error_1 = motor_0_error_0;

    motor_1_error_0 = set_speed_LF - (int)Speed_LF;
    pwm_1 = (int)((motor_1_error_0 - motor_1_error_1) * motor_1_kp + motor_1_error_0 * motor_1_ki + (motor_1_error_0 - 2 * motor_1_error_1 + motor_1_error_2) * motor_1_kd);
    pwm_1_out += pwm_1;
    motor_1_error_2 = motor_1_error_1;
    motor_1_error_1 = motor_1_error_0;

    motor_2_error_0 = set_speed_LB - (int)Speed_LB;
    pwm_2 = (int)((motor_2_error_0 - motor_2_error_1) * motor_2_kp + motor_2_error_0 * motor_2_ki + (motor_2_error_0 - 2 * motor_2_error_1 + motor_2_error_2) * motor_2_kd);
    pwm_2_out += pwm_2;
    motor_2_error_2 = motor_2_error_1;
    motor_2_error_1 = motor_2_error_0;

    motor_3_error_0 = set_speed_RB - (int)Speed_RB;
    pwm_3 = (int)((motor_3_error_0 - motor_3_error_1) * motor_3_kp + motor_3_error_0 * motor_3_ki + (motor_3_error_0 - 2 * motor_3_error_1 + motor_3_error_2) * motor_3_kd);
    pwm_3_out += pwm_3;
    motor_3_error_2 = motor_3_error_1;
    motor_3_error_1 = motor_3_error_0;
}

/**
 * @描述: 外部函数中，需要车身状态发生改变时，调用此函数
 * 重要！！！
 * 车身旋转角度旋转的范围是[-180,180)
 * 不能超过！！！！
 * @传入参数 {int} Speed         车模总速度
 * @传入参数 {float} Angle       车模前进方向与车身y轴夹角
 * @传入参数 {float} Roll_angle  车模旋转到特定角度
 * @返回 {*}
 */
void Set_action(int Speed, float Angle, float Roll_angle)
{
    set_speed = Speed;
    go_angle = Angle;
    Roll_target = Roll_angle;
}
const int center_x = 283, center_y = 315;
int image_x = center_x, image_y = center_y;
float image_error, image_angle , image_error_limit = 10, fix_speed = 0, max_fix_speed = 30;
extern u8 rx_flag;
u8 center_cnt = 0;
u8 filter_num = 50;//滤波次数

//图像数据处理（滤波，转换等等）
static inline void fix_target()
{
	static u8 i = 0;
	if(rx_flag !=0)
	{
		if(rx_flag++>4)//防止误识别
		{
			rx_flag = 0;
			image_x = center_x,image_y = center_y;
		}
	}
	if (action_flag != FIXING)
		return;
	
	if(flag_catch == 1)
	{
		i++;
		if(i == 220)//1000ms的找色块超过就退出
		{
			i = 0;
			flag_catch = 0;
			action_flag = IN_POSITION;
		}
	}
	image_error = sqrt((center_x - image_x) * (center_x - image_x) + (center_y - image_y) * (center_y - image_y));
	image_angle = atan2(center_y - image_y, center_x - image_x) * 180 / 3.1415926;
	if(image_angle > 180)
	{
		image_angle = image_angle - 360;
	}
	else if(image_angle < -180)
	{
		image_angle = image_angle + 360;
	}
	
	if(mode_1)//粗调
	{
		filter_num = 10;
		image_error_limit = 25;
		if(image_error <= 30){max_fix_speed = 50;}
		else{max_fix_speed = 240;}
		fix_speed = 50*image_error+200;
	}
	else if(mode_2)//精调
	{
		filter_num = 30;
		image_error_limit = 6;
		if(image_error <= 15){max_fix_speed = 30;}
		else{max_fix_speed = 185;}
		fix_speed = 40*image_error+20;
	}

	float temp_angle = image_angle;
	int temp_speed = MIN(max_fix_speed, (int)sqrt(fix_speed));//对最高速度进行限制
	if(image_error < image_error_limit)
	{
		if(++center_cnt>filter_num)//找到中心点的次数->进行滤波
		{
			temp_speed = 0;
			action_flag = IN_POSITION;
			center_cnt = 0;
		}
	}
	else
	{
		center_cnt = 0;
	}
	Set_action(temp_speed, temp_angle, Roll_target);
}

/**/
float line_error, line_erro_limit = 3, fix_dis_speed = 0, line_traget = 280;//补光过亮为300   /380
//到达转盘前修正车的位置
static inline void fix_location(void)
{
	if (action_flag != FIXING2)
		return;
	line_error = line_traget - image_y;//求出误差
	fix_dis_speed = line_error * 0.5 + 50;
	int temp_speed = MIN(80,(int)sqrt(fix_dis_speed));
	if(line_error < line_erro_limit)
	{
		temp_speed = 0;
		action_flag = IN_POSITION;
	}
	Set_action(temp_speed,90,Roll_target);//
}

static inline void ZHUNPAN()
{
	if(action_flag != CATCHING1)
		return;
	//判断是否转盘转向
	switch(turn_num)
	{
		case 1:
			TIM_SetCompare2(TIM9,Servo_Turn1);
			action_flag = LEISURE;
			break;
		case 2:
			TIM_SetCompare2(TIM9,Servo_Turn2);
			action_flag = LEISURE;
			break;
		case 3:
			TIM_SetCompare2(TIM9,Servo_Turn3);
			action_flag = LEISURE;
			break;
	}
}

void set_location(float x, float y)
{
	flag_acce = 0;
	motor_0_kp = 20;motor_1_kp = 20;motor_2_kp = 20;motor_3_kp = 20;
	motor_0_ki = 0.18;motor_1_ki = 0.18;motor_2_ki = 0.18;motor_3_ki = 0.18;
	float temp = 0;
	set_x = x;
	set_y = y;
	temp = sqrt((set_x - X_pre)*(set_x - X_pre) + (set_y - Y_pre)*(set_y - Y_pre));
	if(temp >= 100){flagL = 1;flagM = 0;flagS = 0;}
	else if(temp > 30&&temp<100){flagM = 1;flagL = 0;flagS = 0;}
	else if(temp > 0 &&temp<=30){flagL = 0;flagM = 0;flagS = 1;}
	action_flag = WAYING;
}
void set_Roll_Target(float angle)
{
	Roll_target = angle;
	action_flag = ROLLING;
}

static inline void wait_goods_stop()
{
	static int time = 0;
	static int img_last_x;
	static int img_last_y;
	
	if(action_flag != WAITING)
		return;
	
	time++;//计时320ms
	if(time == 65)
	{
		time = 0;
		if(img_last_x <= image_x + 5&& img_last_x >= image_x - 5)
		{
			if(img_last_y <= image_y + 5&& img_last_y >= image_y - 5)
			{
				action_flag = LEISURE;//等待完毕
				return;
			}
		}
		img_last_x = image_x;
		img_last_y = image_y;
		return;
	}
}
//前后左右找圆环 运动周期500ms
static inline void find_circle()
{
	static u8 i = 0;
	static u8 state = 0;
	if(action_flag != MOVEFIND)
		return;
	if(Start_all == 1)
	{
		Start_all = 0;
		if(image_x>=20 && image_x<=620){if(image_y<=460&&image_y>=20){Set_action(0,0,Roll_target);action_flag = FIXING;return;}}//开启修正
	}
	i++;
	if(i==25)//500ms变一次
	{
		i = 0;
		if(state == 0){Set_action(30,80,Roll_target);state=1;return;}
		else if(state == 1){Set_action(-5,180,Roll_target);state=2;return;}
		else if(state == 2){Set_action(+5,180,Roll_target);state=3;return;}
		else if(state == 3){Set_action(20,-80,Roll_target);state=0;return;}
	}
	return;
}

void Control_5ms()
{
	static int times = 0;
//		Start();//开启全部,但转向环并未关闭
		ZHUNPAN();//转盘转动
		wait_goods_stop();//等待物料到来
//		find_circle();//左右移动找环
    Roll_PID_Control();//转向环一直存在
    Get_Tspeed();//小车速度分解给四个轮子的速度
		Roll_to_Target();//转向的状态机
		times++;
		if(times==6)//30ms控制周期
		{
			times = 0;
			switch(Color_Num)
			{
				case 0:
					image_y = image_save[0][0];
					image_x = image_save[0][1];
					rx0 = 0;
					break;
				case 1:
					image_y = image_save[1][0];
					image_x = image_save[1][1];
					rx1 = 0;
					break;
				case 2:
					image_y = image_save[2][0];
					image_x = image_save[2][1];
					rx2 = 0;
					break;
				case 3:
					image_y = image_save[3][0];
					image_x = image_save[3][1];
					rx3 = 0;
					break;
				case 4:
					image_y = image_save[4][0];
					image_x = image_save[4][1];
					rx4 = 0;
					break;
				case 5:
					image_y = image_save[5][0];
					image_x = image_save[5][1];
					rx5 = 0;
					break;
			}
			fix_target();//小车修正状态机
		}
		
//		fix_location();//小车修正位置状态机
		get_to_position();//小车位置环状态机
    Get_speed(0.70);//获取小车速度
    Get_xyspeed();//对速度进行修正
    Updata_location();//更新位置
    PID_Control();
    pwm_out();
}

/*
中断回调函数
*/

void AutoCallBack(void)
{
	static float i=0;
	
	LED0=!LED0;
	Get_Angle();	//获取角度数据
	Control_5ms();
	//判断是否识别二维码数据
	if(recv_ok == 1&&open_QR_flag==1)
	{
		recv_ok = 0;
		open_QR_flag = 0;
		send_num_to_screen = 1;
	}
	i++;
	if(i==80)//0.4s对数据判断是否清除
	{
		i = 0;
		for(int j = 0;j<=5;j++)
		{
			
		}
	}
}

/*锁死停车*/
void LOCK_Car(void)
{
	motor_0_kp = 30;motor_1_kp = 30;motor_2_kp = 30;motor_3_kp = 30;
	motor_0_ki = 1;motor_1_ki = 1;motor_2_ki = 1;motor_3_ki = 1;
	Set_action(0,90,Roll_target);
}
/*
机械臂选转控制
x=0 后->前
x=1 前->后
*/
void Arm_turn(u8 x)
{
  u8 i = 0;
	int temp=0;
	int out =0;
	if(x==0)
	{
		for(i=0;i<=22;i++)
		{
			temp = i*5;
			out = 186-temp;//out倒计时？
			if(out<=76){out=76;}else if(out>=186){out=186;}//对out限幅
			
			TIM_SetCompare1(TIM9,out);	//对TIM9输出out
			if(i<=6||i>=16){delay_ms(35);}//开始等待+结束等待？？
			else{delay_ms(DTime);}
		}
	}
	if(x==1)
	{
		for(i=0;i<=22;i++)
		{
			temp = i*5;
			out = 186-temp;//out倒计时？？
			if(out<=76){out=76;}else if(out>=186){out=186;}//out限幅
			
			TIM_SetCompare1(TIM9,76+temp);//对TIM9输出
			if(i<=6||i>=16){delay_ms(35);}
			else{delay_ms(DTime);}
		}
	}
}


